/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer/sensor/internal/ordered_multi_queue.h"

#include <algorithm>
#include <sstream>
#include <vector>

#include "absl/memory/memory.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace sensor
    {
        namespace
        {
            // Number of items that can be queued up before we log which queues are waiting for data.
            const int kMaxQueueSize = 500;
        } // namespace

        inline std::ostream &operator<<(std::ostream &out, const QueueKey &key)
        {
            return out << '(' << key.trajectory_id << ", " << key.sensor_id << ')';
        }

        // https://blog.csdn.net/weixin_43013761/article/details/127956898

        OrderedMultiQueue::OrderedMultiQueue()
        {
        }

        OrderedMultiQueue::~OrderedMultiQueue()
        {
            for (auto &entry : queues_)
            {
                CHECK(entry.second.finished);
            }
        }

        /// <summary>
        /// 新增轨迹时调用
        /// </summary>
        /// <param name="queue_key"></param>
        /// <param name="callback"></param>
        void OrderedMultiQueue::AddQueue(const QueueKey &queue_key, Callback callback)
        {
            CHECK_EQ(queues_.count(queue_key), 0);
            queues_[queue_key].callback = std::move(callback);
        }

        void OrderedMultiQueue::MarkQueueAsFinished(const QueueKey &queue_key)
        {
            auto it = queues_.find(queue_key);
            CHECK(it != queues_.end()) << "Did not find '" << queue_key << "'.";
            auto &queue = it->second;
            CHECK(!queue.finished);
            queue.finished = true;
            Dispatch();
        }

        /// <summary>
        /// 添加一条最新的传感器数据同时将队列里缓存的传感器数据分发出去，通过调用回调函数将传感器数据传递给轨迹构建器
        /// </summary>
        /// <param name="queue_key"></param>
        /// <param name="data"></param>
        void OrderedMultiQueue::Add(const QueueKey &queue_key, std::unique_ptr<Data> data)
        {
            auto it = queues_.find(queue_key);
            if (it == queues_.end())
            {
                LOG_EVERY_N(WARNING, 1000)
                    << "Ignored data for queue: '" << queue_key << "'";
                return;
            }
            it->second.queue.Push(std::move(data));
            Dispatch();
        }

        void OrderedMultiQueue::Flush()
        {
            std::vector<QueueKey> unfinished_queues;
            for (auto &entry : queues_)
            {
                if (!entry.second.finished)
                {
                    unfinished_queues.push_back(entry.first);
                }
            }
            for (auto &unfinished_queue : unfinished_queues)
            {
                MarkQueueAsFinished(unfinished_queue);
            }
        }

        QueueKey OrderedMultiQueue::GetBlocker() const
        {
            CHECK(!queues_.empty());
            return blocker_;
        }

        /*
        * 将一条轨迹里的所有传感器数据按照时间排序分发，比如一个轨迹(id=0)有scan,imu,odom，其数据时间如下({}括号里的值)
        queues_:
            (0, scan): {      4,     }
            (0, imu):  {1,  3,   5,  }
            (0, odom): {  2,       6,}
        那么分发器会按照1 2 3 4 5 6的时间顺序依次分发出去，所以前端建图接收的输入数据都是按照时间顺序排列的
        */
        void OrderedMultiQueue::Dispatch()
        {
            while (true)
            {
                const Data *next_data = nullptr; // 接下来要处理数据的指针
                Queue *next_queue = nullptr;     // 接下来要处理数据所在队列
                QueueKey next_queue_key;         // 接下来要处理数据所在队列的key
                                                 // 遍历所有的数据队列,找一个时间最小的数据作为要分发的数据；next_data记录上一次循环的队列里最早的数据；
                                                 // 而data是当前循环的队列里面的最早的数据，通过时间对比将最早的数据赋给next_data，继续遍历后面的队列；
                                                 // 最终next_data就是所有队列里面时间最早的数据
                for (auto it = queues_.begin(); it != queues_.end();)
                {
                    // 队列里的第一个数据
                    const auto *data = it->second.queue.Peek<Data>();
                    if (data == nullptr)
                    {
                        if (it->second.finished)
                        {
                            // 该队列已经结束，删除该队列
                            queues_.erase(it++);
                            continue;
                        }
                        // 该队列数据用完了，还没有新数据传送过来
                        CannotMakeProgress(it->first);
                        return;
                    }
                    // 当前队列的第一个数据比上一次循环取出来的数据的时间还小,那么就把这个data当做next_data
                    if (next_data == nullptr || data->GetTime() < next_data->GetTime())
                    {
                        next_data = data;
                        next_queue = &it->second;
                        next_queue_key = it->first;
                    }
                    CHECK_LE(last_dispatched_time_, next_data->GetTime())
                        << "Non-sorted data added to queue: '" << it->first << "'";
                    ++it;
                }
                if (next_data == nullptr)
                {
                    CHECK(queues_.empty());
                    return;
                }
				// 每个队列都有一个唯一的公共起始时间，拿这个next_data与这个时间比较并确定本次的分发时间
                // If we haven't dispatched any data for this trajectory yet, fast forward
                // all queues of this trajectory until a common start time has been reached.
                const common::Time common_start_time = GetCommonStartTime(next_queue_key.trajectory_id);
                if (next_data->GetTime() >= common_start_time)
                {
                    // 正常情况, 数据时间都超过common_start_time
                    // Happy case, we are beyond the 'common_start_time' already.
                    last_dispatched_time_ = next_data->GetTime();
                    next_queue->callback(next_queue->queue.Pop());
                }
                else if (next_queue->queue.Size() < 2)
                {
                    if (!next_queue->finished)
                    {
                        // We cannot decide whether to drop or dispatch this yet.
                        CannotMakeProgress(next_queue_key);
                        return;
                    }
                    last_dispatched_time_ = next_data->GetTime();
                    next_queue->callback(next_queue->queue.Pop());
                }
                else
                {
                    // We take a peek at the time after next data. If it also is not beyond
                    // 'common_start_time' we drop 'next_data', otherwise we just found the
                    // first packet to dispatch from this queue.
                    std::unique_ptr<Data> next_data_owner = next_queue->queue.Pop();
                    if (next_queue->queue.Peek<Data>()->GetTime() > common_start_time)
                    {
                        last_dispatched_time_ = next_data->GetTime();
                        next_queue->callback(std::move(next_data_owner));
                    }
                }
            }
        }

        // 标记queue_key为阻塞者,并按条件发布log,等等这个数据
        void OrderedMultiQueue::CannotMakeProgress(const QueueKey &queue_key)
        {
            blocker_ = queue_key;
            for (auto &entry : queues_)
            {
                if (entry.second.queue.Size() > kMaxQueueSize)
                {
                    LOG_EVERY_N(WARNING, 60) << "Queue waiting for data: " << queue_key;
                    return;
                }
            }
        }

        /// @brief 找到数据队列所有第一帧的最大时间(共同时间),一个轨迹的common_start_time只会计算一次
        /// @param trajectory_id
        /// @return
        common::Time OrderedMultiQueue::GetCommonStartTime(const int trajectory_id)
        {
            auto emplace_result = common_start_time_per_trajectory_.emplace(trajectory_id, common::Time::min());
            common::Time &common_start_time = emplace_result.first->second;
            if (emplace_result.second)
            {
                for (auto &entry : queues_)
                {
                    if (entry.first.trajectory_id == trajectory_id)
                    {
                        common_start_time = std::max(common_start_time, entry.second.queue.Peek<Data>()->GetTime());
                    }
                }
                LOG(INFO) << "All sensor data for trajectory " << trajectory_id
                          << " is available starting at '" << common_start_time << "'.";
            }
            return common_start_time;
        }

    } // namespace sensor
} // namespace cartographer
